#include <ros/ros.h>  
#include <iostream>
#include <sensor_msgs/LaserScan.h>

using namespace std;

ros::Publisher pubber;
ros::Subscriber subber;

double THRESHOLD = 0.13;  // [m]

void laser_callback(const sensor_msgs::LaserScan &laser){
    sensor_msgs::LaserScan msg = laser;
    for(auto& range : msg.ranges){
        if(range < THRESHOLD) range = INFINITY;
    }
    msg.range_min = *min_element(msg.ranges.begin(),msg.ranges.end());
    pubber.publish(msg);
}

int main(int argc, char **argv)  
{
    ros::init(argc, argv, "my_lidar_test");  
    ros::NodeHandle nh;
    pubber = nh.advertise<sensor_msgs::LaserScan>("/scan", 1);
    subber = nh.subscribe("/scan_org", 1, laser_callback);
    ros::spin(); 
}
